#include "bmp180.h"
#include "math.h"

#define BMP_DELAY(ms) HAL_Delay(ms)

static uint8_t oversampling;
static int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
static uint16_t ac4, ac5, ac6;

static bmp180_val_t bmp180_val = {0};

static uint8_t read_byte(uint8_t a)
{
    uint8_t ret = 0;

    soft_i2c_start(BMP085_I2C_ID, BMP085_I2CADDR);
    soft_i2c_read(BMP085_I2C_ID, a, &ret, 1);
    soft_i2c_stop(BMP085_I2C_ID);

    return ret;
}

static uint16_t read_half_word(uint8_t a)
{
    uint16_t ret;
    uint8_t buf[2] = {0};

    soft_i2c_start(BMP085_I2C_ID, BMP085_I2CADDR);
    soft_i2c_read(BMP085_I2C_ID, a, buf, 2);
    soft_i2c_stop(BMP085_I2C_ID);

    ret = buf[0];
    ret <<= 8;
    ret |= buf[1];

    return ret;
}

static void write_byte(uint8_t a, uint8_t d)
{
    uint8_t buf[2];
    soft_i2c_start(BMP085_I2C_ID, BMP085_I2CADDR);
    soft_i2c_write(BMP085_I2C_ID, a, &d, 1);
    soft_i2c_stop(BMP085_I2C_ID);
}

bool bmp180_init(uint8_t mode)
{
    if (mode > BMP085_ULTRAHIGHRES)
        mode = BMP085_ULTRAHIGHRES;
    oversampling = mode;

    soft_i2c_init(BMP085_I2C_ID);

    if (read_byte(0xD0) != 0x55)
        return false;

    /* read calibration data */
    ac1 = read_half_word(BMP085_CAL_AC1);
    ac2 = read_half_word(BMP085_CAL_AC2);
    ac3 = read_half_word(BMP085_CAL_AC3);
    ac4 = read_half_word(BMP085_CAL_AC4);
    ac5 = read_half_word(BMP085_CAL_AC5);
    ac6 = read_half_word(BMP085_CAL_AC6);

    b1 = read_half_word(BMP085_CAL_B1);
    b2 = read_half_word(BMP085_CAL_B2);

    mb = read_half_word(BMP085_CAL_MB);
    mc = read_half_word(BMP085_CAL_MC);
    md = read_half_word(BMP085_CAL_MD);

    return true;
}

int32_t computeB5(int32_t UT)
{
    int32_t X1 = (UT - (int32_t)ac6) * ((int32_t)ac5) >> 15;
    int32_t X2 = ((int32_t)mc << 11) / (X1 + (int32_t)md);
    return X1 + X2;
}

uint16_t read_raw_temperature(void)
{
    write_byte(BMP085_CONTROL, BMP085_READTEMPCMD);
    BMP_DELAY(5);
    return read_half_word(BMP085_TEMPDATA);
}

uint32_t read_raw_pressure(void)
{
    uint32_t raw;

    write_byte(BMP085_CONTROL, BMP085_READPRESSURECMD + (oversampling << 6));

    if (oversampling == BMP085_ULTRALOWPOWER)
        BMP_DELAY(5);
    else if (oversampling == BMP085_STANDARD)
        BMP_DELAY(8);
    else if (oversampling == BMP085_HIGHRES)
        BMP_DELAY(14);
    else
        BMP_DELAY(26);

    raw = read_half_word(BMP085_PRESSUREDATA);

    raw <<= 8;
    raw |= read_byte(BMP085_PRESSUREDATA + 2);
    raw >>= (8 - oversampling);
    return raw;
}

int32_t read_pressure(void)
{
    int32_t UT, UP, B3, B5, B6, X1, X2, X3, p;
    uint32_t B4, B7;

    UT = read_raw_temperature();
    UP = read_raw_pressure();
    B5 = computeB5(UT);

    // 计算气压
    B6 = B5 - 4000;
    X1 = ((int32_t)b2 * ((B6 * B6) >> 12)) >> 11;
    X2 = ((int32_t)ac2 * B6) >> 11;
    X3 = X1 + X2;
    B3 = ((((int32_t)ac1 * 4 + X3) << oversampling) + 2) / 4;

    X1 = ((int32_t)ac3 * B6) >> 13;
    X2 = ((int32_t)b1 * ((B6 * B6) >> 12)) >> 16;
    X3 = ((X1 + X2) + 2) >> 2;
    B4 = ((uint32_t)ac4 * (uint32_t)(X3 + 32768)) >> 15;
    B7 = ((uint32_t)UP - B3) * (uint32_t)(50000UL >> oversampling);

    if (B7 < 0x80000000)
    {
        p = (B7 * 2) / B4;
    }
    else
    {
        p = (B7 / B4) * 2;
    }
    X1 = (p >> 8) * (p >> 8);
    X1 = (X1 * 3038) >> 16;
    X2 = (-7357 * p) >> 16;

    p = p + ((X1 + X2 + (int32_t)3791) >> 4);

    return p;
}

int32_t calc_sealevel_pressure(float pressure, float altitude_meters)
{
    return (int32_t)(pressure / pow(1.0f - altitude_meters / 44330, 5.255f));
}

float read_temperature(void)
{
    int32_t UT, B5; // following ds convention
    float temp;
    UT = read_raw_temperature();
    B5 = computeB5(UT);
    temp = (B5 + 8) >> 4;
    temp /= 10;

    return temp;
}

float calc_altitude(float pressure, float sealevelPressure)
{
    float altitude;

    altitude = 44330 * (1.0f - pow(pressure / sealevelPressure, (float)0.1903f));

    return altitude;
}

void bmp180_update()
{
    bmp180_val.temperature = read_temperature();
    bmp180_val.pressure = read_pressure();
    bmp180_val.sealevelPressure = calc_sealevel_pressure(bmp180_val.pressure, 0);
    bmp180_val.altitude = calc_altitude(bmp180_val.pressure, 101325);
}

void bmp180_get_val(bmp180_val_t *val)
{
    if (val)
        memcpy(val, &bmp180_val, sizeof(bmp180_val_t));
}